/**
 * @file BasicModel.cpp
 * This is a class to compute the basic parts of a GNSS model, i.e.:
 * Geometric distance, relativity correction, satellite position and
 * velocity at transmission time, satellite elevation and azimuth, etc.
 */

//============================================================================
//
//  Dagoberto Salazar - gAGE ( http://www.gage.es ). 2007, 2008, 2009, 2011
// 
//  2020/03/20 shjzhang, modify the initilization method
//
//============================================================================


#include "BasicModel.hpp"
#include "Rinex3EphemerisStore2.hpp"
#include "YDSTime.hpp"
#include "constants.hpp"
#include "DataStructures.hpp"

using namespace std;

#define debug 0

namespace gpstk
{


      // Return a string identifying this object.
    std::string BasicModel::getClassName() const
    { return "BasicModel"; }



      /* Explicit constructor taking as input reference
       * station coordinates.
       *
       * Those coordinates may be Cartesian (X, Y, Z in meters) or Geodetic
       * (Latitude, Longitude, Altitude), but defaults to Cartesian.
       *
       * Also, a pointer to GeoidModel may be specified, but default is
       * NULL (in which case WGS84 values will be used).
       *
       * @param aRx   first coordinate [ X(m), or latitude (degrees N) ]
       * @param bRx   second coordinate [ Y(m), or longitude (degrees E) ]
       * @param cRx   third coordinate [ Z, height above ellipsoid or
       *              radius, in meters ]
       * @param s     coordinate system (default is Cartesian, may be set
       *              to Geodetic).
       * @param ell   pointer to EllipsoidModel
       * @param frame Reference frame associated with this position
       */
    BasicModel::BasicModel( const double& aSta,
                            const double& bSta,
                            const double& cSta,
                            Position::CoordinateSystem s,
                            EllipsoidModel *ell,
                            ReferenceFrame frame )
    {

        minElev = 10.0;
        nominalPos = Position(aSta, bSta, cSta, s, ell, frame);
        pEphStore = NULL;
        pMSCStore = NULL;
        defaultObsOfGPS = TypeID::C1;
        defaultObsOfGAL = TypeID::C1;
        defaultObsOfBDS = TypeID::C2;
        useTGDOfGPS = false;
        useTGDOfGAL = false;
        useTGDOfBDS = false;

    }  // End of 'BasicModel::BasicModel()'


      // Explicit constructor, taking as input a Position object
      // containing reference station coordinates.
    BasicModel::BasicModel(const Position& staPos)
    {

        minElev = 10.0;
        nominalPos = staPos;
        pEphStore = NULL;
        pMSCStore = NULL;
        defaultObsOfGPS = TypeID::C1;
        defaultObsOfGAL = TypeID::C1;
        defaultObsOfBDS = TypeID::C2;
        useTGDOfGPS = false;
        useTGDOfGAL = false;
        useTGDOfBDS = false;

    }  // End of 'BasicModel::BasicModel()'



      /* Explicit constructor, taking as input reference station
       * coordinates, ephemeris to be used, default observable
       * and whether TGD will be computed or not.
       *
       * @param RxCoordinates Reference station coordinates.
       * @param dEphemeris    EphemerisStore object to be used by default.
       * @param dObservable   Observable type to be used by default.
       * @param applyTGD      Whether or not C1 observable will be
       *                      corrected from TGD effect or not.
       *
       */
    BasicModel::BasicModel( const Position& StaPos,
                            XvtStore<SatID>& ephStore,
                            MSCStore* pCoordStore,
                            const TypeID& dObsOfGPS,
                            const TypeID& dObsOfGAL,
                            const TypeID& dObsOfBDS,
                            const bool& applyTGDOfGPS,
                            const bool& applyTGDOfGAL,
                            const bool& applyTGDOfBDS )
    {
        minElev = 10.0;
        nominalPos = StaPos;
        pEphStore = &ephStore;
        pMSCStore = pCoordStore;
        defaultObsOfGPS = dObsOfGPS;
        defaultObsOfGAL = dObsOfGAL;
        defaultObsOfBDS = dObsOfBDS;
        useTGDOfGPS = applyTGDOfGPS;
        useTGDOfGAL = applyTGDOfGAL;
        useTGDOfBDS = applyTGDOfBDS;

    }  // End of 'BasicModel::BasicModel()'



      /* Return a satTypeValueMap object, adding the new data generated when
       * calling a modeling object.
       *
       * @param time      Epoch.
       * @param gData     Data object holding the data.
       */
    satTypeValueMap& BasicModel::Process( const CommonTime& time,
                                          const SourceID& source,
                                          satTypeValueMap& gData )
            noexcept(false)
    {

        try
        {
            SatIDSet satRejectedSet;
            TypeID defaultObs;

            ///////////////
            // get satellite xyz
            ///////////////

            // Loop through all the satellites
            for(satTypeValueMap::iterator it = gData.begin();
                it != gData.end();
                ++it)
            {
                SatID sat( it->first );

                try
                {
                   Xvt svPosVel;

                      // compute satellite ephemeris at transmitting time
                      // Scalar to hold temporal value
                   double obs(0.0);
                   if(sat.system == SatelliteSystem::GPS)
                   {
                       defaultObs = defaultObsOfGPS;
                   }
                   else if(sat.system == SatelliteSystem::Galileo)
                   {
                       defaultObs = defaultObsOfGAL;
                   }
                   else if(sat.system == SatelliteSystem::BDS)
                   {
                       defaultObs = defaultObsOfBDS;
                   }

                   obs = (*it).second(defaultObs);

                   svPosVel = ComputeAtTransmitTime(time, obs, sat);

                   //
                   // transmitting-time related parameters
                   //

                   // relativity
                   double relativity(0.0);
                   relativity = svPosVel.computeRelativityCorrection()*C_MPS;

                   // clock bias, clock drift
                   double svclkbias(0.0), svclkdrift(0.0);
                   svclkbias = svPosVel.clkbias*C_MPS;
                   svclkdrift = svPosVel.clkdrift*C_MPS;

                   (*it).second[TypeID::relativity] = -relativity;
                   // Let's insert satellite clock bias at transmit time
                   (*it).second[TypeID::cdtSat] = svclkbias;
                   (*it).second[TypeID::cdtSatDot] = svclkdrift;

                   // earth-rotaion-related values

                   // Let's insert satellite position at transmit time
                   (*it).second[TypeID::satXECF] = svPosVel.x[0];
                   (*it).second[TypeID::satYECF] = svPosVel.x[1];
                   (*it).second[TypeID::satZECF] = svPosVel.x[2];

                   // Let's insert satellite velocity at transmit time
                   (*it).second[TypeID::satVXECF] = svPosVel.v[0];
                   (*it).second[TypeID::satVYECF] = svPosVel.v[1];
                   (*it).second[TypeID::satVZECF] = svPosVel.v[2];

                }
                catch(InvalidRequest& e)
                {
                    // If some problem appears, then schedule this satellite
                    // for removal
                    satRejectedSet.insert( sat );

                    // exception should not be rethrow again, 2019, shjzhang
/*
                    GPSTK_RETHROW(e);
*/
                    continue;
                }


            } // End of loop for(satTypeValueMap = gData.begin()...

            //////////////////
            // if some satellite is not found in ephemeris, you have to 
            // remove the satellite before linearize.
            //////////////////

            // Remove satellites with missing data
            gData.removeSatID(satRejectedSet);


            CommonTime epoch = time;
            epoch.setTimeSystem( TimeSystem::Unknown );

            int gpsNumSats = gData.numSats(SatelliteSystem::GPS);

            if(gpsNumSats < 4)
            {
                SVNumException s("BasicModel:: GPS number is less than 4!");
                GPSTK_THROW(s);
            }

            if(pMSCStore!=NULL)
            {
                MSCData mscData;
                try
                {
                    mscData = pMSCStore->findMSC(source.sourceName,epoch);
                }
                catch(...)
                {
                    cout << "BasicModel::source not found in MSCStore!" << endl;
                    exit(-1);
                }
                nominalPos = mscData.coordinates;
            }
            else if(pEpochXvStore!=NULL)
            {
                Triple rcvPos(0.0, 0.0, 0.0);
                try
                {
                    rcvPos = pEpochXvStore->getPos(epoch);
                    nominalPos[0] = rcvPos[0];
                    nominalPos[1] = rcvPos[1];
                    nominalPos[2] = rcvPos[2];
                }
                catch(...)
                {
                    nominalPos = bancroftSolution(epoch, gData);
                }
            }
            else
            {
                if(kinematic)
                {
                    nominalPos = bancroftSolution(epoch, gData);
                }
                // for static receiver, only one nominalPos for all epochs is needed.
                else
                {
                    if(firstEpoch)
                    {
                        nominalPos = bancroftSolution(epoch, gData);
                    }
                }
            }


            if(debug)
            {
                if(kinematic)
                {
                    bancPos = bancroftSolution(epoch, gData);
                }
                // for static receiver, only one nominalPos for all epochs is needed.
                else
                {
                    if(firstEpoch)
                    {
                        bancPos = bancroftSolution(epoch, gData);
                    }
                }

                double diff = range(nominalPos, bancPos);; 

                if(diff>1000)
                {
                    cout << "bancroft error!" << endl;
                }
                    
                Position diffPos = nominalPos - bancPos ;

                cout << "nominalPos: " << nominalPos << endl;
                cout << "bancroft Pos: " << bancPos << endl;
                cout << "diffPos: " << diffPos << endl;
                cout << "kinematic: " << kinematic << endl;
                cout << "time: " << time << endl;

            }


            // 
            // now, compute earth-rotation and receiver-position-related values
            //

            // Loop through all the satellites
            for(satTypeValueMap::iterator it = gData.begin();
                it != gData.end();
                ++it)
            {
                Xvt svPosVel;
                SatID sat( it->first );

                Triple svPos( (*it).second[TypeID::satXECF],
                              (*it).second[TypeID::satYECF],
                              (*it).second[TypeID::satZECF]);

                Triple svVel( (*it).second[TypeID::satVXECF],
                              (*it).second[TypeID::satVYECF],
                              (*it).second[TypeID::satVZECF]);

                svPosVel.x = svPos;
                svPosVel.v = svVel;

                rotateEarth(svPosVel);

                // rawrange
                double rawrange(0.0);
                rawrange = RSS(svPosVel.x[0] - nominalPos.X(),
                               svPosVel.x[1] - nominalPos.Y(),
                               svPosVel.x[2] - nominalPos.Z());

                // partials
                Triple cosines;
                cosines[0] = (nominalPos.X() - svPosVel.x[0]) / rawrange;
                cosines[1] = (nominalPos.Y() - svPosVel.x[1]) / rawrange;
                cosines[2] = (nominalPos.Z() - svPosVel.x[2]) / rawrange;

                // elevation, azimuth
                double elevation(0.0), azimuth(0.0);
                double elevationGeodetic(0.0), azimuthGeodetic(0.0);

                Position SV( svPosVel );
                try
                {
                    elevation = nominalPos.elevation(SV);
                    azimuth = nominalPos.azimuth(SV);
                    elevationGeodetic = nominalPos.elevationGeodetic(SV);
                    azimuthGeodetic = nominalPos.azimuthGeodetic(SV);
                }
                catch(Exception& e)
                {
                    cerr << e << endl;
                    continue;
                }

                // Let's test if satellite has enough elevation over horizon
                if ( elevationGeodetic < minElev )
                {
                    // Mark this satellite if it doesn't have enough elevation
                    satRejectedSet.insert( sat );
                    continue;
                }

                // update rotation-related values

                // Let's insert satellite position at transmit time
                (*it).second[TypeID::satXECF] = svPosVel.x[0];
                (*it).second[TypeID::satYECF] = svPosVel.x[1];
                (*it).second[TypeID::satZECF] = svPosVel.x[2];

                // Let's insert satellite velocity at transmit time
                (*it).second[TypeID::satVXECF] = svPosVel.v[0];
                (*it).second[TypeID::satVYECF] = svPosVel.v[1];
                (*it).second[TypeID::satVZECF] = svPosVel.v[2];

                // receiver-position-related values

                // rho, relativity, elevation, azimuth
                (*it).second[TypeID::rho] = rawrange;
                (*it).second[TypeID::elevation] = elevationGeodetic;
                (*it).second[TypeID::azimuth] = azimuthGeodetic;

                // Let's insert partials for station position at receive time
                (*it).second[TypeID::dStaX] = cosines[0];
                (*it).second[TypeID::dStaY] = cosines[1];
                (*it).second[TypeID::dStaZ] = cosines[2];

                // constant values

                // Let's insert partials for station clock bias at receive time
                if(sat.system == SatelliteSystem::GPS)
                {
                    (*it).second[TypeID::dcdtSta] = 1.0;
                }
                else if(sat.system == SatelliteSystem::Galileo)
                {
                    (*it).second[TypeID::dcdtStaGAL] = 1.0;
                }
                else if(sat.system == SatelliteSystem::BDS)
                {
                    (*it).second[TypeID::dcdtStaBDS] = 1.0;
                }

            } // End of loop for(satTypeValueMap = gData.begin()...

            // Remove satellites with missing data
            gData.removeSatID(satRejectedSet);

            firstEpoch = false;

            return gData;

        }   // End of try...
        catch(SVNumException& u)
        {
            GPSTK_THROW(u);
        }
        catch(Exception& u)
        {
            // Throw an exception if something unexpected happens
            ProcessingException e( getClassName() + ":" + u.what() );
            GPSTK_THROW(u);
        }

    }  // End of method 'BasicModel::Process()'

    void BasicModel::Process(gnssRinex& gData)
            noexcept(false)
    {
        try
        {
            Process( gData.header.epoch, gData.header.source,  gData.body );

            // store the nominalPos to header
            gData.header.srcPos = nominalPos;
        }
        catch(SVNumException& e)
        {
            GPSTK_THROW(e);
        }
        catch(Exception& e)
        {
            GPSTK_THROW(e);
        }

    }  // End of method 'BasicModel::Process()'


    void BasicModel::Process(commonHeader& comHeader, gnssRinex& gData)
            noexcept(false)
    {
        pEphStore = comHeader.pSP3Store;
        pMSCStore = comHeader.pMSCStore;
        pRefSys   = comHeader.pRefSys;

        Process( gData.header.epoch, gData.header.source,  gData.body );

        // store the nominalPos to header
        gData.header.srcPos = nominalPos;


    }  // End of method 'BasicModel::Process()'


     /** Return a sourceRinex object, adding the new data generated when
      *  calling a modeling object.
      *
      * @param gData    Data object holding the data.
      */
    void BasicModel::Process(commonHeader& comHeader, sourceRinex& gData)
            noexcept(false)
    {
        pEphStore = comHeader.pSP3Store;
        pMSCStore = comHeader.pMSCStore;
        pRefSys   = comHeader.pRefSys;

        SourceIDSet sourceRejectedSet;

        CommonTime epoch( gData.header.epoch );

        epoch.setTimeSystem( TimeSystem::Unknown );

        for(sourceDataMap::iterator sdmIt = gData.body.begin();
            sdmIt != gData.body.end();
            ++sdmIt)
        {
            SourceID source( sdmIt->first );

            if(pMSCStore == NULL)
            {
                sourceRejectedSet.insert( source );
                continue;
            }

            string station( source.sourceName );
            MSCData mscData;
            try
            {
                mscData = pMSCStore->findMSC(station,epoch);
            }
            catch(...)
            {
                sourceRejectedSet.insert( source );
                continue;
            }

            nominalPos = mscData.coordinates;

            Process( gData.header.epoch, source, sdmIt->second );


        }
        
        gData.removeSourceID( sourceRejectedSet );


    }  // End of method 'BasicModel::Process()'

    Xvt BasicModel::ComputeAtTransmitTime(const CommonTime& tr,
                                           const double& pr,
                                           const SatID& sat)
    {
        Xvt svPosVel;

        CommonTime tt;
        CommonTime transmit( tr );
        transmit -= pr/C_MPS;
        tt = transmit;

        for(int i=0; i<2; i++)
        {
            try
            {
                if(pEphStore==NULL)
                {
                    cerr << "pEphStore must be given" << endl;
                    exit(-1);
                }
                else
                {
                    svPosVel = pEphStore->getXvt(sat,tt);
                }
            }
            catch(InvalidRequest& e)
            {
                GPSTK_RETHROW(e);
            }

            tt = transmit;
            tt -= (svPosVel.clkbias + svPosVel.relcorr);
        }

        return svPosVel;

    };

    void BasicModel::rotateEarth(Xvt& svPosVel)
    {
        double tof(0.0);

        // rotate from ECF at transmit time to ECF at receive time
        tof = RSS(svPosVel.x[0] - nominalPos.X(),
                  svPosVel.x[1] - nominalPos.Y(),
                  svPosVel.x[2] - nominalPos.Z())/C_MPS;

        double wt(0.0);
        wt = OMEGA_EARTH * tof;

        double sx(0.0), sy(0.0);

        sx = +std::cos(wt)*svPosVel.x[0] + std::sin(wt)*svPosVel.x[1];
        sy = -std::sin(wt)*svPosVel.x[0] + std::cos(wt)*svPosVel.x[1];

        svPosVel.x[0] = sx;
        svPosVel.x[1] = sy;

        sx = +std::cos(wt)*svPosVel.v[0] + std::sin(wt)*svPosVel.v[1];
        sy = -std::sin(wt)*svPosVel.v[0] + std::cos(wt)*svPosVel.v[1];

        svPosVel.v[0] = sx;
        svPosVel.v[1] = sy;
    }

    Position BasicModel::bancroftSolution(const CommonTime& time,
                                          satTypeValueMap& gData) 
    {

        Vector<double> gpsObsVec;
        Vector<double> bdsObsVec;
        Vector<double> galObsVec;

        gpsObsVec = gData.getVectorOfTypeID(TypeID::PC12, SatelliteSystem::GPS);
        bdsObsVec = gData.getVectorOfTypeID(TypeID::PC27, SatelliteSystem::BDS);
        galObsVec = gData.getVectorOfTypeID(TypeID::PC15, SatelliteSystem::Galileo);

        if(debug)
        {
            cout << "obsVec:" << endl;
            cout << gpsObsVec << endl;
            cout << bdsObsVec << endl;
            cout << galObsVec << endl;
        }

        Vector<double> gpsSatClkVec;
        Vector<double> bdsSatClkVec;
        Vector<double> galSatClkVec;

        gpsSatClkVec = gData.getVectorOfTypeID(TypeID::cdtSat, SatelliteSystem::GPS);
        bdsSatClkVec = gData.getVectorOfTypeID(TypeID::cdtSat, SatelliteSystem::BDS);
        galSatClkVec = gData.getVectorOfTypeID(TypeID::cdtSat, SatelliteSystem::Galileo);

        if(debug)
        {
            cout << "clkVec:" << endl;
            cout << gpsSatClkVec << endl;
            cout << bdsSatClkVec << endl;
            cout << galSatClkVec << endl;
        }

        Matrix<double> gpsSatXYZMatrix;
        Matrix<double> bdsSatXYZMatrix;
        Matrix<double> galSatXYZMatrix;

        TypeIDSet xyzSet;
        xyzSet.insert(TypeID::satXECF);
        xyzSet.insert(TypeID::satYECF);
        xyzSet.insert(TypeID::satZECF);

        gpsSatXYZMatrix = gData.getMatrixOfTypes(xyzSet, SatelliteSystem::GPS);
        bdsSatXYZMatrix = gData.getMatrixOfTypes(xyzSet, SatelliteSystem::BDS);
        galSatXYZMatrix = gData.getMatrixOfTypes(xyzSet, SatelliteSystem::Galileo);

        if(debug)
        {
            cout << "xyzVec:" << endl;
            cout << gpsSatXYZMatrix << endl;
            cout << bdsSatXYZMatrix << endl;
            cout << galSatXYZMatrix << endl;
        }

        ///////////////////// /////////////////////
        // ATTENTION please
        // obs = obs + cdtSat;
        // we will add all satellite clock to the PC observables
        ///////////////////// /////////////////////

        int gpsValidNum(0);
        int bdsValidNum(0);
        int galValidNum(0);
        if( gpsObsVec.size() == gpsSatClkVec.size() && 
                gpsObsVec.size() == gpsSatXYZMatrix.rows() )
        {
            gpsValidNum = gpsObsVec.size();
            gpsObsVec += gpsSatClkVec;
        }

        if( bdsObsVec.size() == bdsSatClkVec.size() && 
                bdsObsVec.size() == bdsSatXYZMatrix.rows() )
        {
            bdsValidNum = bdsObsVec.size();
            bdsObsVec += bdsSatClkVec;
        }

        if( galObsVec.size() == galSatClkVec.size() && 
                galObsVec.size() == galSatXYZMatrix.rows() )
        {
            galValidNum = galObsVec.size();
            galObsVec += galSatClkVec;
        }


        if(debug)
        {
            cout << "obsvec:" << endl;
            cout << gpsObsVec << endl;
            cout << bdsObsVec << endl;
            cout << galObsVec << endl;

            cout << "valid num" << endl;
            cout << gpsValidNum << endl;
            cout << galValidNum << endl;
            cout << bdsValidNum << endl;
        }

        /////////// ///////////
        // bancroft solution
        /////////// ///////////
        Vector<double> gpsSolution(4, 0.0);
        Vector<double> bdsSolution(4, 0.0);
        Vector<double> galSolution(4, 0.0);

        ///////// ///////// ///////// ///////// ///////// ///////// ///////// 
        // only return one selected solution given the criteraion
        //
        // warning: you should check the results for LEO satellite
        //
        ///////// ///////// ///////// ///////// ///////// ///////// ///////// 
        bancroft.ChooseOne = true;

        bool gpsIsValid(false);
        if(gpsValidNum>0)
        {
            Matrix<double> dataMatrixGPS(gpsValidNum, 4, 0.0);

            for(int i=0;i<gpsValidNum;i++)
            {
                dataMatrixGPS(i,0) = gpsSatXYZMatrix(i,0);
                dataMatrixGPS(i,1) = gpsSatXYZMatrix(i,1);
                dataMatrixGPS(i,2) = gpsSatXYZMatrix(i,2);
                dataMatrixGPS(i,3) = gpsObsVec(i);
            }

            if( bancroft.Compute(dataMatrixGPS, gpsSolution) == 0 )
            {
                gpsIsValid = true;
            }
        }

        bool bdsIsValid(false);
        if(bdsValidNum>0)
        {
            Matrix<double> dataMatrixBDS(bdsValidNum, 4, 0.0);
            for(int i=0;i<bdsValidNum;i++)
            {
                dataMatrixBDS(i,0) = bdsSatXYZMatrix(i,0);
                dataMatrixBDS(i,1) = bdsSatXYZMatrix(i,1);
                dataMatrixBDS(i,2) = bdsSatXYZMatrix(i,2);
                dataMatrixBDS(i,3) = bdsObsVec(i);
            }

            if( bancroft.Compute(dataMatrixBDS, bdsSolution) == 0)
            {
                bdsIsValid = true;
            }
        }

        bool galIsValid(false);
        if(galValidNum>0)
        {
            Matrix<double> dataMatrixGAL(galValidNum, 4, 0.0);
            for(int i=0;i<galValidNum;i++)
            {
                dataMatrixGAL(i,0) = galSatXYZMatrix(i,0);
                dataMatrixGAL(i,1) = galSatXYZMatrix(i,1);
                dataMatrixGAL(i,2) = galSatXYZMatrix(i,2);
                dataMatrixGAL(i,3) = galObsVec(i);
            }
            if( bancroft.Compute(dataMatrixGAL, galSolution) == 0) 
            {
                galIsValid = true;
            }

        }
        
        // currently only return GPS soltuions

        Position gpsPos;
        gpsPos.setECEF(gpsSolution(0), gpsSolution(1), gpsSolution(2));

        Position bdsPos;
        bdsPos.setECEF(bdsSolution(0), bdsSolution(1), bdsSolution(2));

        Position galPos;
        galPos.setECEF(galSolution(0), galSolution(1), galSolution(2));

        int numValidPos(0);
        Position meanPos;
        meanPos.setECEF(0,0,0);

        if(gpsIsValid)
        {
            meanPos+=gpsPos;
            numValidPos++;
        }

        if(bdsIsValid)
        {
            meanPos+=bdsPos;
            numValidPos++;
        }

        if(galIsValid)
        {
            meanPos+=galPos;
            numValidPos++;
        }
        
        cout << "gpsPos:" << gpsPos << endl;
        cout << "bdsPos:" << bdsPos << endl;
        cout << "galPos:" << galPos << endl;

        double x = meanPos[0]/numValidPos; 
        double y = meanPos[1]/numValidPos; 
        double z = meanPos[2]/numValidPos; 

        Position recPos;
        recPos.setECEF(x,y,z);

        return recPos;

    }


}  // End of namespace gpstk
